% SLAM参数优化工具

clear all;
close all;
clc;

fprintf('SLAM参数优化工具\n');
fprintf('1.快速测试  2.网格搜索  3.贝叶斯优化  4.敏感度分析  5.完整优化\n');
mode = input('模式[1-5]: ');
if isempty(mode), mode = 1; end

fprintf('目标: 1.精度  2.覆盖  3.综合  4.稳定性\n');
optimization_goal = input('目标[1-4]: ');
if isempty(optimization_goal), optimization_goal = 3; end

%% 参数空间
param_space = struct();
param_space.Q_std_values = [
    0.005, 0.005, 0.002;
    0.010, 0.010, 0.005;
    0.015, 0.015, 0.007;
    0.020, 0.020, 0.010;
    0.030, 0.030, 0.015;
    0.050, 0.050, 0.025;
    0.075, 0.075, 0.035;
    0.100, 0.100, 0.050
];
param_space.R_std_values = [
    0.05, 0.02;
    0.08, 0.03;
    0.10, 0.05;
    0.15, 0.07;
    0.20, 0.08;
    0.25, 0.10;
    0.30, 0.12
];
param_space.range_values = [10, 12, 15, 18, 20, 25, 30];
param_space.speed_values = [0.8, 1.0, 1.2, 1.5, 1.8, 2.0, 2.5];
param_space.test_time_values = [60, 80, 100];

config = struct();
config.n_runs = 3;
config.test_time = 80;
config.dt = 0.1;
config.enable_convergence = true;

%% 执行优化
results = [];
start_time = tic;

switch mode
    case 1
        % 快速测试模式
        results = run_quick_test(param_space, config, optimization_goal);
        
    case 2
        % 网格搜索模式
        results = run_grid_search(param_space, config, optimization_goal);
        
    case 3
        % 智能优化模式（贝叶斯优化）
        results = run_bayesian_optimization(param_space, config, optimization_goal);
        
    case 4
        % 参数敏感度分析
        run_sensitivity_analysis(param_space, config);
        return;  % 敏感度分析后直接返回
        
    case 5
        % 完整优化
        results = run_full_optimization(param_space, config, optimization_goal);
end

total_time = toc(start_time);

%% 结果
fprintf('\n优化完成 (耗时: %.1f分钟)\n', total_time/60);
[~, sorted_idx] = sort([results.score]);
results = results(sorted_idx);

fprintf('\n前3个配置:\n');
for i = 1:min(3, length(results))
    r = results(i);
    fprintf('#%d Q=[%.3f,%.3f,%.3f] R=[%.3f,%.3f] range=%.1f speed=%.2f\n', ...
            i, r.Q_std, r.R_std, r.max_range, r.v_base);
    fprintf('   误差=%.3f±%.3f 发现率=%.1f%% 得分=%.3f\n', ...
            r.avg_error, r.std_error, r.discovery_rate, r.score);
end

best = results(1);
fprintf('\n推荐参数:\n');
fprintf('Q = diag([%.3f, %.3f, %.3f]).^2;\n', best.Q_std);
fprintf('R = diag([%.3f, %.3f]).^2;\n', best.R_std);
fprintf('max_range = %.1f;\n', best.max_range);
fprintf('v_base = %.2f;\n', best.v_base);

create_visualization(results, optimization_goal);
save('optimization_results.mat', 'results', 'param_space', 'config');
fprintf('结果已保存到 optimization_results.mat\n');
generate_report(results, param_space, config, optimization_goal, total_time);

%% ========================================================================
%% 辅助函数
%% ========================================================================

function results = run_quick_test(param_space, config, opt_goal)
    fprintf('\n快速测试\n');
    test_configs = {
        [1, 1, 4, 4], [2, 3, 5, 4], [3, 3, 5, 5], [4, 3, 6, 4], [3, 2, 5, 3]
    };
    results = [];
    for i = 1:length(test_configs)
        fprintf('配置%d/%d...', i, length(test_configs));
        result = evaluate_config(test_configs{i}, param_space, config, opt_goal);
        results = [results; result];
        fprintf(' 误差%.3f 发现率%.1f%%\n', result.avg_error, result.discovery_rate);
    end
end

function results = run_grid_search(param_space, config, opt_goal)
    fprintf('\n网格搜索\n');
    Q_indices = [1, 2, 3, 4, 5];
    R_indices = [1, 2, 3, 4];
    range_indices = [2, 3, 4, 5, 6];
    speed_indices = [2, 3, 4, 5];
    total = length(Q_indices) * length(R_indices) * length(range_indices) * length(speed_indices);
    fprintf('共%d个配置\n', total);
    results = [];
    count = 0;
    for q_idx = Q_indices
        for r_idx = R_indices
            for rng_idx = range_indices
                for spd_idx = speed_indices
                    count = count + 1;
                    fprintf('%d/%d (%.0f%%)...', count, total, 100*count/total);
                    result = evaluate_config([q_idx, r_idx, rng_idx, spd_idx], param_space, config, opt_goal);
                    results = [results; result];
                    fprintf(' 得分%.3f\n', result.score);
                end
            end
        end
    end
end

function results = run_bayesian_optimization(param_space, config, opt_goal)
    fprintf('\n贝叶斯优化\n');
    n_initial = 8;
    n_iterations = 20;
    results = [];
    fprintf('阶段1:初始采样(%d)\n', n_initial);
    for i = 1:n_initial
        cfg = [randi(size(param_space.Q_std_values, 1)), ...
               randi(size(param_space.R_std_values, 1)), ...
               randi(length(param_space.range_values)), ...
               randi(length(param_space.speed_values))];
        fprintf('样本%d/%d...', i, n_initial);
        result = evaluate_config(cfg, param_space, config, opt_goal);
        results = [results; result];
        fprintf(' 得分%.3f\n', result.score);
    end
    fprintf('阶段2:迭代优化(%d)\n', n_iterations);
    for iter = 1:n_iterations
        [~, sorted_idx] = sort([results.score]);
        base_cfg = results(sorted_idx(1)).config_idx;
        cfg = base_cfg;
        param_to_change = randi(4);
        switch param_to_change
            case 1
                cfg(1) = max(1, min(size(param_space.Q_std_values, 1), base_cfg(1) + randi(3) - 2));
            case 2
                cfg(2) = max(1, min(size(param_space.R_std_values, 1), base_cfg(2) + randi(3) - 2));
            case 3
                cfg(3) = max(1, min(length(param_space.range_values), base_cfg(3) + randi(3) - 2));
            case 4
                cfg(4) = max(1, min(length(param_space.speed_values), base_cfg(4) + randi(3) - 2));
        end
        fprintf('迭代%d/%d...', iter, n_iterations);
        result = evaluate_config(cfg, param_space, config, opt_goal);
        results = [results; result];
        fprintf(' 得分%.3f (最优%.3f)\n', result.score, min([results.score]));
    end
end

function run_sensitivity_analysis(param_space, config)
    fprintf('\n敏感度分析\n');
    base_cfg = [3, 3, 4, 4];
    figure('Position', [100, 100, 1400, 800]);
    
    subplot(2,2,1);
    errors_Q = [];
    for i = 1:size(param_space.Q_std_values, 1)
        cfg = base_cfg; cfg(1) = i;
        result = evaluate_config(cfg, param_space, config, 1);
        errors_Q(i) = result.avg_error;
        fprintf('Q%d: %.3f\n', i, result.avg_error);
    end
    plot(errors_Q, 'o-', 'LineWidth', 2, 'MarkerSize', 8);
    xlabel('运动噪声级别'); ylabel('误差(m)'); title('Q影响'); grid on;
    
    subplot(2,2,2);
    errors_R = [];
    for i = 1:size(param_space.R_std_values, 1)
        cfg = base_cfg; cfg(2) = i;
        result = evaluate_config(cfg, param_space, config, 1);
        errors_R(i) = result.avg_error;
        fprintf('R%d: %.3f\n', i, result.avg_error);
    end
    plot(errors_R, 's-', 'LineWidth', 2, 'MarkerSize', 8);
    xlabel('观测噪声级别'); ylabel('误差(m)'); title('R影响'); grid on;
    
    subplot(2,2,3);
    errors_range = [];
    discovery_range = [];
    for i = 1:length(param_space.range_values)
        cfg = base_cfg; cfg(3) = i;
        result = evaluate_config(cfg, param_space, config, 1);
        errors_range(i) = result.avg_error;
        discovery_range(i) = result.discovery_rate;
        fprintf('Range%d: %.3f, %.1f%%\n', i, result.avg_error, result.discovery_rate);
    end
    yyaxis left;
    plot(param_space.range_values, errors_range, 'd-', 'LineWidth', 2, 'MarkerSize', 8);
    ylabel('误差(m)');
    yyaxis right;
    plot(param_space.range_values, discovery_range, '^-', 'LineWidth', 2, 'MarkerSize', 8);
    ylabel('发现率(%)');
    xlabel('范围(m)'); title('Range影响'); grid on;
    legend('误差', '发现率', 'Location', 'best');
    
    subplot(2,2,4);
    errors_speed = [];
    for i = 1:length(param_space.speed_values)
        cfg = base_cfg; cfg(4) = i;
        result = evaluate_config(cfg, param_space, config, 1);
        errors_speed(i) = result.avg_error;
        fprintf('Speed%d: %.3f\n', i, result.avg_error);
    end
    plot(param_space.speed_values, errors_speed, 'v-', 'LineWidth', 2, 'MarkerSize', 8);
    xlabel('速度(m/s)'); ylabel('误差(m)'); title('Speed影响'); grid on;
    fprintf('\n分析完成\n');
end

function results = run_full_optimization(param_space, config, opt_goal)
    fprintf('\n完整优化\n');
    fprintf('第1轮:粗搜索\n');
    config.n_runs = 2;
    results1 = run_grid_search(param_space, config, opt_goal);
    fprintf('\n第2轮:精细搜索\n');
    config.n_runs = 3;
    [~, sorted_idx] = sort([results1.score]);
    best3 = results1(sorted_idx(1:min(3, length(results1))));
    results2 = [];
    for i = 1:length(best3)
        base = best3(i).config_idx;
        for dq = -1:1
            for dr = -1:1
                cfg = base;
                cfg(1) = max(1, min(size(param_space.Q_std_values, 1), base(1) + dq));
                cfg(2) = max(1, min(size(param_space.R_std_values, 1), base(2) + dr));
                result = evaluate_config(cfg, param_space, config, opt_goal);
                results2 = [results2; result];
            end
        end
    end
    results = [results1; results2];
    unique_configs = unique(vertcat(results.config_idx), 'rows');
    unique_results = [];
    for i = 1:size(unique_configs, 1)
        for j = 1:length(results)
            if isequal(results(j).config_idx, unique_configs(i, :))
                unique_results = [unique_results; results(j)];
                break;
            end
        end
    end
    results = unique_results;
end

%% 核心函数：评估单个配置
function result = evaluate_config(cfg, param_space, config, opt_goal)
    % 提取参数
    Q_std = param_space.Q_std_values(cfg(1), :);
    R_std = param_space.R_std_values(cfg(2), :);
    max_range = param_space.range_values(cfg(3));
    v_base = param_space.speed_values(cfg(4));
    
    % 多次运行
    all_errors = [];
    all_max_errors = [];
    all_discovery_rates = [];
    all_convergence_times = [];
    
    for run = 1:config.n_runs
        [avg_err, max_err, disc_rate, conv_time] = ...
            run_single_slam(Q_std, R_std, max_range, v_base, config);
        
        all_errors = [all_errors, avg_err];
        all_max_errors = [all_max_errors, max_err];
        all_discovery_rates = [all_discovery_rates, disc_rate];
        all_convergence_times = [all_convergence_times, conv_time];
    end
    
    % 统计结果
    result.config_idx = cfg;
    result.Q_std = Q_std;
    result.R_std = R_std;
    result.max_range = max_range;
    result.v_base = v_base;
    result.avg_error = mean(all_errors);
    result.std_error = std(all_errors);
    result.max_error = mean(all_max_errors);
    result.discovery_rate = mean(all_discovery_rates);
    result.convergence_time = mean(all_convergence_times);
    
    % 计算综合得分
    result.score = calculate_score(result, opt_goal);
end

%% 运行单次SLAM仿真
function [avg_error, max_error, discovery_rate, convergence_time] = ...
         run_single_slam(Q_std, R_std, max_range, v_base, config)
    
    % 构造协方差矩阵
    Q = diag(Q_std).^2;
    R = diag(R_std).^2;
    
    % 仿真参数
    dt = config.dt;
    total_time = config.test_time;
    steps = round(total_time / dt);
    
    % 地图（与main_slam.m相同）
    landmarks = [
        10, 10; 30, 10; 30, 30; 10, 30; 20, 20;
        40, 15; 40, 35; 15, 40; 35, 5; 5, 15;
        15, 15; 25, 15; 25, 25; 15, 25; 35, 15;
        35, 25; 5, 25; 20, 35
    ];
    n_landmarks = size(landmarks, 1);
    
    % 初始化
    true_state = [0; 0; 0];
    estimated_state = [0; 0; 0];
    P = diag([0.01, 0.01, 0.001]);
    
    fov = 2 * pi;
    omega_base = 0.08;
    
    true_trajectory = zeros(3, steps);
    estimated_trajectory = zeros(3, steps);
    observed_landmarks = [];
    position_errors = zeros(1, steps);
    
    % 边界设置
    boundary = struct();
    boundary.x_min = -5;
    boundary.x_max = 45;
    boundary.y_min = -5;
    boundary.y_max = 45;
    boundary.margin = 1.5;
    
    boundary_avoiding = false;
    avoidance_timer = 0;
    angle_diff = 0;
    v = v_base;
    omega = omega_base;
    
    convergence_threshold = 0.5;  % 0.5m误差视为收敛
    convergence_time = total_time;  % 默认值
    converged = false;
    
    % 主循环
    for t = 1:steps
        % 边界检测
        x = true_state(1);
        y = true_state(2);
        theta = true_state(3);
        
        near_x_min = (x - boundary.x_min) < boundary.margin;
        near_x_max = (boundary.x_max - x) < boundary.margin;
        near_y_min = (y - boundary.y_min) < boundary.margin;
        near_y_max = (boundary.y_max - y) < boundary.margin;
        
        at_boundary = near_x_min || near_x_max || near_y_min || near_y_max;
        
        if at_boundary && ~boundary_avoiding
            boundary_avoiding = true;
            avoidance_timer = 50;
            
            target_theta = theta;
            if near_x_min
                target_theta = 0;
            elseif near_x_max
                target_theta = pi;
            end
            if near_y_min
                target_theta = pi/2;
            elseif near_y_max
                target_theta = -pi/2;
            end
            
            angle_diff = wrapToPi(target_theta - theta);
        end
        
        % 生成控制输入（使用bounded_random模式）
        if boundary_avoiding && avoidance_timer > 0
            v = v_base * 0.5;
            omega = sign(angle_diff) * 0.3;
            avoidance_timer = avoidance_timer - 1;
            if avoidance_timer == 0
                boundary_avoiding = false;
            end
        else
            if mod(t, 40) == 0
                rng(t + round(sum(Q_std)*1000));  % 确保每次运行不同
                v = v_base * (0.8 + 0.4 * rand());
                omega = omega_base * (-3 + 6 * rand());
            end
        end
        
        % 运动和观测
        control = [v; omega];
        true_state = robot_motion(true_state, control, dt, Q);
        
        [observations, observed_ids] = sensor_observation(true_state, landmarks, ...
                                                           max_range, fov, R);
        
        % EKF
        [estimated_state, P] = ekf_predict(estimated_state, P, control, dt, Q);
        
        for i = 1:length(observed_ids)
            landmark_id = observed_ids(i);
            z = observations(:, i);
            
            if ~ismember(landmark_id, observed_landmarks)
                [estimated_state, P] = initialize_landmark(estimated_state, P, z, R);
                observed_landmarks = [observed_landmarks; landmark_id];
            else
                map_idx = find(observed_landmarks == landmark_id);
                [estimated_state, P] = ekf_update(estimated_state, P, z, map_idx, R);
            end
        end
        
        % 记录
        true_trajectory(:, t) = true_state;
        estimated_trajectory(:, t) = estimated_state(1:3);
        position_errors(t) = norm(true_state(1:2) - estimated_state(1:2));
        
        % 检测收敛
        if ~converged && t > 100  % 前10秒不算
            recent_errors = position_errors(max(1, t-50):t);
            if mean(recent_errors) < convergence_threshold
                converged = true;
                convergence_time = t * dt;
            end
        end
    end
    
    % 计算指标
    avg_error = mean(position_errors);
    max_error = max(position_errors);
    discovery_rate = 100 * length(observed_landmarks) / n_landmarks;
end

%% 计算综合得分
function score = calculate_score(result, opt_goal)
    switch opt_goal
        case 1  % 精度优先
            score = result.avg_error * 1.0 + result.std_error * 0.3;
            
        case 2  % 覆盖优先
            score = (100 - result.discovery_rate) * 0.1;
            
        case 3  % 综合优化
            norm_error = result.avg_error / 2.0;  % 假设2m是可接受误差
            norm_discovery = (100 - result.discovery_rate) / 100;
            norm_stability = result.std_error / 0.5;  % 假设0.5m是可接受标准差
            score = 0.5 * norm_error + 0.3 * norm_discovery + 0.2 * norm_stability;
            
        case 4  % 稳定性优先
            score = result.convergence_time * 0.01 + result.std_error * 2.0;
    end
end

%% 创建可视化
function create_visualization(results, opt_goal)
    figure('Position', [100, 100, 1400, 900]);
    
    % 子图1：误差分布
    subplot(2,3,1);
    errors = [results.avg_error];
    histogram(errors, 15, 'FaceColor', [0.2 0.6 0.8]);
    xlabel('平均位置误差 (m)');
    ylabel('配置数量');
    title('误差分布直方图');
    grid on;
    
    % 子图2：发现率分布
    subplot(2,3,2);
    discovery = [results.discovery_rate];
    histogram(discovery, 15, 'FaceColor', [0.8 0.4 0.2]);
    xlabel('路标发现率 (%)');
    ylabel('配置数量');
    title('发现率分布直方图');
    grid on;
    
    % 子图3：误差vs发现率散点图
    subplot(2,3,3);
    scatter(errors, discovery, 50, [results.score], 'filled');
    xlabel('平均位置误差 (m)');
    ylabel('路标发现率 (%)');
    title('误差-发现率关系');
    colorbar;
    colormap('jet');
    grid on;
    
    % 子图4：前10配置对比
    subplot(2,3,4);
    top10_errors = errors(1:min(10, length(errors)));
    bar(top10_errors, 'FaceColor', [0.3 0.7 0.5]);
    xlabel('配置排名');
    ylabel('平均位置误差 (m)');
    title('前10配置误差对比');
    grid on;
    
    % 子图5：参数分布（最优配置的参数）
    subplot(2,3,5);
    top_n = min(20, length(results));
    ranges = [results(1:top_n).max_range];
    speeds = [results(1:top_n).v_base];
    scatter(ranges, speeds, 100, 1:top_n, 'filled');
    xlabel('传感器范围 (m)');
    ylabel('运动速度 (m/s)');
    title('最优配置的参数分布');
    colorbar;
    grid on;
    
    % 子图6：收敛时间
    subplot(2,3,6);
    if isfield(results, 'convergence_time')
        conv_times = [results.convergence_time];
        top10_conv = conv_times(1:min(10, length(conv_times)));
        bar(top10_conv, 'FaceColor', [0.7 0.3 0.7]);
        xlabel('配置排名');
        ylabel('收敛时间 (s)');
        title('前10配置收敛速度');
        grid on;
    end
    
    sgtitle('SLAM参数优化结果可视化', 'FontSize', 14, 'FontWeight', 'bold');
end

function generate_report(results, param_space, config, opt_goal, total_time)
    fid = fopen('optimization_report.txt', 'w');
    fprintf(fid, 'SLAM参数优化报告\n');
    fprintf(fid, '时间: %s\n', datestr(now));
    fprintf(fid, '耗时: %.1f分钟\n', total_time/60);
    fprintf(fid, '配置数: %d\n\n', length(results));
    
    best = results(1);
    fprintf(fid, '最优配置:\n');
    fprintf(fid, 'Q = diag([%.3f, %.3f, %.3f]).^2;\n', best.Q_std);
    fprintf(fid, 'R = diag([%.3f, %.3f]).^2;\n', best.R_std);
    fprintf(fid, 'max_range = %.1f;\n', best.max_range);
    fprintf(fid, 'v_base = %.2f;\n\n', best.v_base);
    fprintf(fid, '性能: 误差=%.3f±%.3f, 发现率=%.1f%%, 得分=%.3f\n\n', ...
            best.avg_error, best.std_error, best.discovery_rate, best.score);
    
    for i = 2:min(5, length(results))
        r = results(i);
        fprintf(fid, '#%d 得分%.3f: Q=[%.3f,%.3f,%.3f] R=[%.3f,%.3f] range=%.1f speed=%.2f\n', ...
                i, r.score, r.Q_std, r.R_std, r.max_range, r.v_base);
    end
    fclose(fid);
    fprintf('报告已保存到 optimization_report.txt\n');
end
